/*
Autor: Alexander Savochkin, 2010
New BSD License
*/

#include "TwoChannelSecondOrderModel.h"
#include "OperatedInputFactory.h"
#include "OperatedOutputFactory.h"

#include <cmath>
#include <boost/lexical_cast.hpp>

using std::sin;
using std::cos;
using boost::lexical_cast;

using SRCLF::Config::Section;
using SRCLF::FactoriesKeeper;
using SRCLF::normal;

namespace SRCLF
{
namespace Demo
{

TwoDimensionSecondOrder::TwoDimensionSecondOrder(const string& resource_id, const Configuration &config) :
	mass(lexical_cast<double>( config.getSection(Section::resource, resource_id).getScalarByKey("mass") ))
{
	Section section = config.getSection(Section::resource, resource_id);
	
	double alpha = lexical_cast<double>( section.getScalarByKey("alpha") );
	cos_alpha = cos(alpha);
	sin_alpha = sin(alpha);
	
	position[0] = 0.0;
	position[1] = 0.0;
	
	velocity[0] = 0.0;
	velocity[1] = 0.0;
	
	force[0] = 0.0;
	force[1] = 0.0;

	BOOST_LOG_SEV(logger, normal) << "TwoDimensionSecondOrder model system object created: alpha = " << alpha << ", mass = " << mass;
}

void TwoDimensionSecondOrder::update(double dt)
{
	array<double,2> acceleration;
	acceleration[0] = (cos_alpha * force[0] - sin_alpha * force[1]) / mass;
	acceleration[1] = (sin_alpha * force[0] + cos_alpha * force[1]) / mass;

	position[0] += velocity[0] * dt + 0.5 * acceleration[0] * dt * dt;
	position[1] += velocity[1] * dt + 0.5 * acceleration[1] * dt * dt;

	velocity[0] += acceleration[0] * dt;
	velocity[1] += acceleration[1] * dt;
}

void TwoDimensionSecondOrder::setForce(int dimension, double _force)
{
	force[dimension] = _force;
}

double TwoDimensionSecondOrder::getForce(int dimension)
{
	return force[dimension];
}

double TwoDimensionSecondOrder::getX(int dimension) const
{
	return position[dimension];
}

double TwoDimensionSecondOrderInput::getInput()
{
	return io_resource->getX( channel_number );
}

void TwoDimensionSecondOrderInput::adminCommand(const string& command, string& result)
{
}

void TwoDimensionSecondOrderInput::controlCommand(const string& command, string& result)
{
}

void TwoDimensionSecondOrderInput::infoCommand(const string& command, string& result)
{
}

void TwoDimensionSecondOrderInput::update(const ptime& new_time)
{
	if ( new_time != last_update) 
	{
		time_duration d = new_time - last_update;
		double dt = 1.e-3 * d.total_milliseconds();
		io_resource->update(dt);
		last_update = new_time;
	}
}

void TwoDimensionSecondOrderOutput::setOutput(double value)
{
	io_resource->setForce( channel_number, value );
}

double TwoDimensionSecondOrderOutput::getOutput()
{
	return io_resource->getForce( channel_number );
}

void TwoDimensionSecondOrderOutput::adminCommand(const string& command, string& result)
{
}

void TwoDimensionSecondOrderOutput::controlCommand(const string& command, string& result)
{
}

void TwoDimensionSecondOrderOutput::infoCommand(const string& command, string& result)
{
}

bool registered1 = FactoriesKeeper::getInstance().registerInputFactory("mechanical_model_input",
			static_cast<SRCLF::OperatedInputFactory*>(new SRCLF::IO::DefaultOperatedInputFactory<TwoDimensionSecondOrderInput>()));

bool registered2 = FactoriesKeeper::getInstance().registerOutputFactory("mechanical_model_output",
			static_cast<SRCLF::OperatedOutputFactory*>(new SRCLF::IO::DefaultOperatedOutputFactory<TwoDimensionSecondOrderOutput>()));

} //namespace Demo
} //namespace SRCLF
